#pragma once
#include "mpc.h"
#include <iostream>
#include <yaml-cpp/yaml.h>
#include <chrono>
#include <string>
#include "vehicle_dyn_factory.hpp"
#include "kf_factory.hpp"
#include <fstream>
#include "planning_ref_point.hpp"
#include "linear_steady_state.h"
#include <map>
using namespace cvx;
namespace auto_ros
{
namespace control
{
class VehicleLMpcBase
{
public:
	VehicleLMpcBase(const std::string dyn_mpc_kf_config_path,
					const std::string vehicle_lmpc_yaml_name, double rate);
	virtual void init_mpc() = 0;
	virtual bool step(const Eigen::VectorXd &x0, const Eigen::VectorXd &u_fb,
					  const PlanningRefPoint &ref_point, double speed);
	virtual bool step(const Eigen::VectorXd &x0, const Eigen::VectorXd &u_fb,
					  const PlanningRefPoint &ref_point, double speed,
					  const std::map<std::string, double> other_input);
	virtual bool step(const Eigen::VectorXd &x0,
					  const Eigen::VectorXd &u_fb,
					  const std::vector<PlanningRefPoint> &ref_point,
					  double speed);

	virtual bool step(const Eigen::VectorXd &x0,
					  const Eigen::VectorXd &u_fb, double kappa, double speed);
	virtual bool reset_kf() = 0;
	virtual bool reset_kf(const Eigen::VectorXd &state_esti) = 0;
	std::shared_ptr<Mpc> vehicle_mpc_ptr_;
	std::shared_ptr<VehicleDynBase> vehicle_dyn_ptr_;
	std::shared_ptr<DiscreteLinearSys> dis_dyn_ptr_;
	std::shared_ptr<KfVehicleBase> kf_lateral_ptr_;
	YAML::Node mpc_yaml();
	Eigen::VectorXd x_ref_;
	Eigen::VectorXd u_ref_;
	YAML::Node mpc_yaml_;
	auto_ros::common_tool::double_map log_map_;

protected:
	double rate_;
	std::shared_ptr<LinearSteadyState> ref_state_slover_ptr_;
	auto_ros::common_tool::double_map dynpar_map_;
};

} // namespace control
} // namespace auto_ros